#ifdef KITSCHY_DEBUG_MEMORY 
#include "debug_memorymanager.h"
#endif

#ifdef WIN32
#include "windows.h"
#else
#include <sys/time.h>
#include <time.h>
#endif
#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include "stdarg.h"
#include "math.h"

#include "BList.h"

#include "symbol.h"
#include "sort.h"
#include "ontology.h"
#include "featureterm.h"
#include "FTKBase.h"
#include "Ufeatureterm.h"
#include "FT_refinement.h"
#include "FT_generalization.h"

#include "Planning.h"

void validInstantiationsAuxiliar(FeatureTerm *op,FeatureTerm *state,List<FeatureTerm> *objects,FTKBase *dm,int alreadyMatched,List<FeatureTerm> *result);
FeatureTerm *executeEffect(FeatureTerm *effect,FeatureTerm *s,FTKBase *dm);
FeatureTerm *executeNegatedEffect(FeatureTerm *effect,FeatureTerm *s,FTKBase *dm);
List<FeatureTerm> *getUnsatisfiedPredicatesAuxiliar(FeatureTerm *state,FeatureTerm *goal,FTKBase *dm,List<FeatureTerm> *l);
List<FeatureTerm> *getUnsatisfiedPredicatesAuxiliarNegated(FeatureTerm *state,FeatureTerm *goal,FTKBase *dm,List<FeatureTerm> *l);


Sort *and_sort=0,*or_sort=0,*not_sort=0,*when_sort=0,*unless_sort=0,*if_sort=0,*operator_sort=0;


void initPlanning(Ontology *o)
{
	and_sort=o->get_sort("and");
	or_sort=o->get_sort("or");
	not_sort=o->get_sort("not");
	when_sort=o->get_sort("when");
	unless_sort=o->get_sort("unless");
	if_sort=o->get_sort("if");
	operator_sort=o->get_sort("operator");
} /* initPlanning */ 



List<FeatureTerm> *validInstantiations(FTKBase *dm,List<FeatureTerm> *objects,FeatureTerm *state)
{
	List<FeatureTerm> *operators = dm->SearchFT(operator_sort);
	FeatureTerm *f;
	List<FeatureTerm> *res = new List<FeatureTerm>;
	List<FeatureTerm> *tmp_res;

	operators->Rewind();
	while(operators->Iterate(f)) {
//	if (operators->Iterate(f)) {
//		printf("--- operator %i/%i ---\n",operators->PositionRef(f),operators->Length());
		tmp_res = validInstantiations(f,objects,state,dm);
		while(!tmp_res->EmptyP()) res->Add(tmp_res->ExtractIni());
		delete tmp_res;
	} // while 

	operators->ExtractAll();
	delete operators;

	return res;
} /* validInstantiations */ 


List<FeatureTerm> *validInstantiations(FeatureTerm *op,List<FeatureTerm> *objects,FeatureTerm *state,FTKBase *dm)
{
	List<FeatureTerm> *result = new List<FeatureTerm>;

//	printf("validInstantiations\n");
	validInstantiationsAuxiliar(op,state,objects,dm,0,result);
//	printf("found %i\n",result->Length());

	return result;
} /* validInstantiations */ 


void validInstantiationsAuxiliar(FeatureTerm *op,FeatureTerm *state,List<FeatureTerm> *objects,FTKBase *dm,int alreadyMatched,List<FeatureTerm> *result)
{
	List<FeatureTerm> localObjects;
	FeatureTerm *parameter;
	FeatureTerm *f;
	FeatureTerm *object;
	FeatureTerm *clone;
//	FeatureTerm *match; 

//	printf("validInstantiationsAuxiliar: %i\n",alreadyMatched);

	f = op->featureValueQuick("parameters");
	if (f->isSet()) {
		if (alreadyMatched>=f->set_values_quick()->Length()) {
			if (validOperatorInstantiation(op,state,dm)) result->Add(new FeatureTerm(op));
			return;
		} // if
	} else {
		if (alreadyMatched>=1) {
			if (validOperatorInstantiation(op,state,dm)) result->Add(new FeatureTerm(op));
			return;
		} // if
	} // if

	if (f->isSet()) {
		parameter = f->set_values_quick()->operator [](alreadyMatched);
	} else {
		parameter = f;
	} // if

	localObjects.Instance(*objects);
	localObjects.Rewind();
	while(localObjects.Iterate(object)) {
//		match = simple_unification(object,parameter,state->getSort()->getOntology(),dm);
		if (parameter->subsumes(object)) {
/*
			char *s,*s1,*s2;
			s1 = object->toStringNOOS(dm);
			s2 = parameter->toStringNOOS(dm);
			s = match->toStringNOOS(dm);
			printf("Match:\n%s\nT1: %s\nT2: %s\n",s,s1,s2);
			delete []s;
			delete []s1;
			delete []s2;
*/
			clone = op->clone_substituting(parameter,object);
			clone->setName(0);

//			delete match;

			validInstantiationsAuxiliar(clone,state,objects,dm,alreadyMatched+1,result);

			delete clone;
		} // if
	} // while
} /* validInstantiationsAuxiliar */ 


bool validOperatorInstantiation(FeatureTerm *op,FeatureTerm *state,FTKBase *dm)
{
	FeatureTerm *predicate = op->featureValueQuick("precondition");

	if (predicate!=0) {
		bool retval = evaluatePredicate(predicate,state,dm);
//		printf("validOperatorInstantiation: %s\n",(retval ? "true":"false"));
		return retval;
	} // if
	printf("validOperatorInstantiation: empty predicate!\n");
	return true;
} /* validOperatorInstantiation */ 


bool evaluatePredicate(FeatureTerm *predicate,FeatureTerm *state,FTKBase *dm)
{
	Ontology *o = predicate->getSort()->getOntology();
	if (predicate->getSort()->is_a(and_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicates");
		if (f->isSet()) {
			List<FeatureTerm> l;
			l.Instance(*(f->set_values_quick()));
			while(l.Iterate(f)) {
				if (!evaluatePredicate(f,state,dm)) return false;
			} // while
			return true;
		} else {
			return evaluatePredicate(f,state,dm);
		} //
	} else if (predicate->getSort()->is_a(or_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicates");
		if (f->isSet()) {
			List<FeatureTerm> l;
			l.Instance(*(f->set_values_quick()));
			while(l.Iterate(f)) {
				if (evaluatePredicate(f,state,dm)) return true;
			} // while
			return false;
		} else {
			return evaluatePredicate(f,state,dm);
		} //
	} else if (predicate->getSort()->is_a(not_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicate");
		return !evaluatePredicate(f,state,dm);
	} else if (predicate->getSort()->is_a(when_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("effect");
			return !evaluatePredicate(f,state,dm);
		} else {
			return true;
		} // if		
	} else if (predicate->getSort()->is_a(unless_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (!evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("effect");
			return !evaluatePredicate(f,state,dm);
		} else {
			return true;
		} // if		
	} else if (predicate->getSort()->is_a(if_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("positive-effect");
			return !evaluatePredicate(f,state,dm);
		} else {
			f = predicate->featureValueQuick("negative-effect");
			return !evaluatePredicate(f,state,dm);
		} // if		
	} else {
		if (state->getSort()->is_a(and_sort)) {
			FeatureTerm *f1 = state->featureValueQuick("predicates");
			return predicate->subsumes(f1);
		} else {
			// Game state is not an and of things... not supported yet...
			printf("evaluatePredicate: Game State is not an 'and' construction!\n");
			return false;
		} // if
	} // if
} /* evaluatePredicate */ 


void printOperatorNice(FeatureTerm *op,FTKBase *dm)
{
	FeatureTerm *n = op->featureValueQuick("name");
	FeatureTerm *v = op->featureValueQuick("parameters");

	if (n!=0) {
		printf("%s ",n->getSymbol()->get());
	} else {
		printf("<operator> ");
	} // if 

    if (v!=0) {
        if (v->isSet()) {
            List<FeatureTerm> l;
            l.Instance(*(v->set_values_quick()));
            l.Rewind();
            while(l.Iterate(v)) {
                if (v->getName()!=0) {
                    printf("%s ",v->getName()->get());
                } else {
                    char *s = v->toStringNOOS(dm);
                    printf("%s ",s);
                    delete []s;
                } // if
            } // while 
            printf("\n");
        } else {
            if (v->getName()!=0) {
                printf("%s ",v->getName()->get());
            } else {
                char *s = v->toStringNOOS(dm);
                printf("%s\n",s);
                delete []s;
            } // if
        } // if
    } else {
        printf("\n");
    }
} /* printOperatorNice */ 


FeatureTerm *applyOperator(FeatureTerm *op,FeatureTerm *state,FTKBase *dm)
{
	FeatureTerm *effect = op->featureValueQuick("effect");
	FeatureTerm *tmp = FeatureTerm::SetFeatureTerm();

	tmp->addSetValue(new FeatureTerm(effect));
	tmp->addSetValue(new FeatureTerm(state));

	FeatureTerm *tmp2 = tmp->clone(dm);

	FeatureTerm *effect_clone = new FeatureTerm(tmp2->set_values_quick()->operator [](0));
	FeatureTerm *state_clone = new FeatureTerm(tmp2->set_values_quick()->operator [](1));

	if (!evaluatePredicate(effect_clone,state_clone,dm)) {
		state_clone = executeEffect(effect_clone,state_clone,dm);
	} // if

	delete effect_clone;
	delete tmp2;
	delete tmp;

	return state_clone;
} /* applyOperator */ 


FeatureTerm *executeEffect(FeatureTerm *effect,FeatureTerm *s,FTKBase *dm)
{
	Ontology *o = effect->getSort()->getOntology();

	if (effect->getSort()->is_a(and_sort)) {
		FeatureTerm *f;
		List<FeatureTerm> l;
		l.Instance(*(effect->featureValueQuick("predicates")->set_values_quick()));
		l.Rewind();
		while(l.Iterate(f)) {	
			if (!evaluatePredicate(f,s,dm)) {
				s = executeEffect(f,s,dm);
			} // if
		} // while
	} else if (effect->getSort()->is_a(or_sort)) {
		printf("executeNegatedEffect: effects of an operator has an 'or' construct! Not suppoerted...\n");
	} else if (effect->getSort()->is_a(not_sort)) {
		FeatureTerm *f1 = effect->featureValueQuick("predicate");
		s = executeNegatedEffect(f1,s,dm);
	} else if (effect->getSort()->is_a(when_sort)) {
		FeatureTerm *f = effect->featureValueQuick("condition");
		if (evaluatePredicate(f,s,dm)) {
			f = effect->featureValueQuick("effect");
			s = executeEffect(f,s,dm);			
		} // if		
	} else if (effect->getSort()->is_a(unless_sort)) {
		FeatureTerm *f = effect->featureValueQuick("condition");
		if (evaluatePredicate(f,s,dm)) {
			f = effect->featureValueQuick("effect");
			s = executeEffect(f,s,dm);			
		} // if		
	} else if (effect->getSort()->is_a(if_sort)) {
		FeatureTerm *f = effect->featureValueQuick("condition");
		if (evaluatePredicate(f,s,dm)) {
			f = effect->featureValueQuick("positive-effect");
			s = executeEffect(f,s,dm);			
		} else {
			f = effect->featureValueQuick("negative-effect");
			s = executeEffect(f,s,dm);			
		} // if		
	} else {
		if (s->getSort()->is_a(and_sort)) {
			FeatureTerm *f1 = s->featureValueQuick("predicates");
			f1->addSetValue(new FeatureTerm(effect));
		} else {
			// Game state is not an and of things... not supported yet...
			printf("executeEffect: Game State is not an 'and' construction, cannot add a new predicate!\n");
		} // if
	} // if 

	return s;
} /* executeEffect */ 


FeatureTerm *executeNegatedEffect(FeatureTerm *effect,FeatureTerm *s,FTKBase *dm)
{
	Ontology *o = effect->getSort()->getOntology();

	if (effect->getSort()->is_a(and_sort)) {
		FeatureTerm *f;
		List<FeatureTerm> l;
		l.Instance(*(effect->featureValueQuick("predicates")->set_values_quick()));
		l.Rewind();
		while(l.Iterate(f)) {	
			if (!evaluatePredicate(f,s,dm)) {
				s = executeNegatedEffect(f,s,dm);
			} // if
		} // while
	} else if (effect->getSort()->is_a(or_sort)) {
		printf("executeNegatedEffect: effects of an operator has an 'or' construct! Not suppoerted...\n");
	} else if (effect->getSort()->is_a(not_sort)) {
		FeatureTerm *f1 = effect->featureValueQuick("predicate");
		s = executeNegatedEffect(f1,s,dm);
	} else if (effect->getSort()->is_a(when_sort)) {
		FeatureTerm *f = effect->featureValueQuick("condition");
		if (evaluatePredicate(f,s,dm)) {
			f = effect->featureValueQuick("effect");
			s = executeNegatedEffect(f,s,dm);			
		} // if		
	} else if (effect->getSort()->is_a(unless_sort)) {
		FeatureTerm *f = effect->featureValueQuick("condition");
		if (evaluatePredicate(f,s,dm)) {
			f = effect->featureValueQuick("effect");
			s = executeNegatedEffect(f,s,dm);			
		} // if		
	} else if (effect->getSort()->is_a(if_sort)) {
		FeatureTerm *f = effect->featureValueQuick("condition");
		if (evaluatePredicate(f,s,dm)) {
			f = effect->featureValueQuick("positive-effect");
			s = executeNegatedEffect(f,s,dm);			
		} else {
			f = effect->featureValueQuick("negative-effect");
			s = executeNegatedEffect(f,s,dm);			
		} // if		
	} else {
		if (s->getSort()->is_a(and_sort)) {
			FeatureTerm *f;
			FeatureTerm *f1 = s->featureValueQuick("predicates");
			List<FeatureTerm> l,to_delete;
			l.Instance(*(f1->set_values_quick()));
			l.Rewind();
			while(l.Iterate(f)) {
				if (effect->subsumes(f)) {
					to_delete.Add(f);
				} // if
			} // while

			while(!to_delete.EmptyP()) {
				f = to_delete.ExtractIni();
				f1->set_values_quick()->DeleteElement(f);
				delete f;
			} // while
		} else {
			// Game state is not an and of things... not supported yet...
			printf("executeNegatedEffect: Game State is not an 'and' construction, cannot add a new predicate!\n");
		} // if
	} // if 

	return s;
} /* executeNegatedEffect */ 


List<FeatureTerm> *getUnsatisfiedPredicates(FeatureTerm *state,FeatureTerm *goal,FTKBase *dm)
{
	List<FeatureTerm> *l = new List<FeatureTerm>;
	return getUnsatisfiedPredicatesAuxiliar(state,goal,dm,l);
} /* getUnsatisfiedPredicates */ 


List<FeatureTerm> *getUnsatisfiedPredicatesAuxiliar(FeatureTerm *state,FeatureTerm *predicate,FTKBase *dm,List<FeatureTerm> *l)
{
	Ontology *o = predicate->getSort()->getOntology();
	if (predicate->getSort()->is_a(and_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicates");
		if (f->isSet()) {
			List<FeatureTerm> l2;
			l2.Instance(*(f->set_values_quick()));
			while(l2.Iterate(f)) {
				getUnsatisfiedPredicatesAuxiliar(f,state,dm,l);
			} // while
			return l;
		} else {
			return getUnsatisfiedPredicatesAuxiliar(f,state,dm,l);
		} //
	} else if (predicate->getSort()->is_a(or_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicates");
		if (f->isSet()) {
			List<FeatureTerm> l2;
			l2.Instance(*(f->set_values_quick()));
			while(l2.Iterate(f)) {
				getUnsatisfiedPredicatesAuxiliar(f,state,dm,l);
			} // while
			return l;
		} else {
			return getUnsatisfiedPredicatesAuxiliar(f,state,dm,l);
		} //
	} else if (predicate->getSort()->is_a(not_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicate");
		return getUnsatisfiedPredicatesAuxiliarNegated(f,state,dm,l);
	} else if (predicate->getSort()->is_a(when_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("effect");
			return getUnsatisfiedPredicatesAuxiliar(f,state,dm,l);
		} else {
			return l;
		} // if		
	} else if (predicate->getSort()->is_a(unless_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (!evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("effect");
			return getUnsatisfiedPredicatesAuxiliar(f,state,dm,l);
		} else {
			return l;
		} // if		
	} else if (predicate->getSort()->is_a(if_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("positive-effect");
			return getUnsatisfiedPredicatesAuxiliar(f,state,dm,l);
		} else {
			f = predicate->featureValueQuick("negative-effect");
			return getUnsatisfiedPredicatesAuxiliar(f,state,dm,l);
		} // if		
	} else {
		if (state->getSort()->is_a(and_sort)) {
			FeatureTerm *f1 = state->featureValueQuick("predicates");
			if (!predicate->subsumes(f1)) {
				l->Add(new FeatureTerm(predicate));
			} // if
			return l;
		} else {
			// Game state is not an and of things... not supported yet...
			printf("getUnsatisfiedPredicatesAuxiliar: Game State is not an 'and' construction!\n");
			return l;
		} // if
	} // if
} /* getUnsatisfiedPredicatesAuxiliar */ 



List<FeatureTerm> *getUnsatisfiedPredicatesAuxiliarNegated(FeatureTerm *state,FeatureTerm *predicate,FTKBase *dm,List<FeatureTerm> *l)
{	
	Ontology *o = predicate->getSort()->getOntology();
	if (predicate->getSort()->is_a(and_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicates");
		if (f->isSet()) {
			List<FeatureTerm> l2;
			l2.Instance(*(f->set_values_quick()));
			while(l2.Iterate(f)) {
				getUnsatisfiedPredicatesAuxiliarNegated(f,state,dm,l);
			} // while
			return l;
		} else {
			return getUnsatisfiedPredicatesAuxiliarNegated(f,state,dm,l);
		} //
	} else if (predicate->getSort()->is_a(or_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicates");
		if (f->isSet()) {
			List<FeatureTerm> l2;
			l2.Instance(*(f->set_values_quick()));
			while(l2.Iterate(f)) {
				getUnsatisfiedPredicatesAuxiliarNegated(f,state,dm,l);
			} // while
			return l;
		} else {
			return getUnsatisfiedPredicatesAuxiliarNegated(f,state,dm,l);
		} //
	} else if (predicate->getSort()->is_a(not_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicate");
		return getUnsatisfiedPredicatesAuxiliar(f,state,dm,l);
	} else if (predicate->getSort()->is_a(when_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("effect");
			return getUnsatisfiedPredicatesAuxiliarNegated(f,state,dm,l);
		} else {
			return l;
		} // if		
	} else if (predicate->getSort()->is_a(unless_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (!evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("effect");
			return getUnsatisfiedPredicatesAuxiliarNegated(f,state,dm,l);
		} else {
			return l;
		} // if		
	} else if (predicate->getSort()->is_a(if_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("positive-effect");
			return getUnsatisfiedPredicatesAuxiliarNegated(f,state,dm,l);
		} else {
			f = predicate->featureValueQuick("negative-effect");
			return getUnsatisfiedPredicatesAuxiliarNegated(f,state,dm,l);
		} // if		
	} else {
		if (state->getSort()->is_a(and_sort)) {
			FeatureTerm *f1 = state->featureValueQuick("predicates");
			if (predicate->subsumes(f1)) {
				l->Add(new FeatureTerm(predicate));
			} // if
			return l;
		} else {
			// Game state is not an and of things... not supported yet...
			printf("getUnsatisfiedPredicatesAuxiliarNegated: Game State is not an 'and' construction!\n");
			return l;
		} // if
	} // if
} /* getUnsatisfiedPredicatesAuxiliarNegated */ 
